//simplify2 is new edition of simplify,
//it allows you to operate with multiple outlines: each node is elimineated or leaved in all the outline,
//simplify is leaved intact because it's well tested and commented

#include "outline/simplify2.h"
#include "outline/type_traits.h"
#include "glib/g2/smatrix.h"
#include <limits>

namespace outline {

///node_here[i] describes what would be if destination outline node is in i'th source outline node
struct node_here {
	double d; ///< metrics of approximation discrepancy (-1, if no node is possible here)
	unsigned prev_node; ///< best previous node is coincide with #prev_node source outline node (0, if this step is the first one)

	node_here() { }
	node_here(double dd, unsigned pn) : d(dd), prev_node(pn) { }
};

///The class accumulates information about start, several intermediate, and finish points,
///and provides summed square of discrepancy of intermediate points from line start-finish.
///The class allows easy testing of new finish point (previous finish point becomes else one intermediate point).
template <typename T>
class best_line;

template <typename T>
class best_line< point<T> > {
public:
	typedef typename type_traits<T>::accumulator_type accumulator_type;
	typedef g2::basic_vector<accumulator_type>	vector;
	typedef g2::basic_smatrix<accumulator_type>	smatrix;
public:
	best_line() { }
	best_line(vector const & s)
		{ start(s); }
public:
	///forgets accumulated information and sets new start point of line
	void start(vector const & s)
		{ r0 = s; q.zero(); }
	///what discrepancy would be if finish is in given point?
	double finish(vector const & f) const
		{ return measure(tsquare(f - r0)); }
	///accumulates information about else one intermediate point
	void intermediate(vector const & i)
		{ q += tsquare(i - r0); }
	///the same as sequence of finish(...) value followed by intermediate(...)
	double intermediate_measure(vector const & i) { 
		smatrix sqr_delta = tsquare(i - r0);
		double r = measure(sqr_delta);
		//assert (r >= min_discrepancy());
		q += sqr_delta;
		return r;
	}
	///wherever final point is, discrepancy will not be less than returned value
	double min_discrepancy() const
		{ return g2::smatrix(q).eigenvalue(0); }
	///measures distances from start point (not from any line)
	double discrepancy_from_start() const
		{ return q.tr(); }
private:
	double measure(smatrix const & sqr_delta) const
		{ return q.tr() - double(dscalar_product(q, sqr_delta)) / sqr_delta.tr(); }
private:
	vector r0;
	smatrix q;
};

/// best lines for all the channels
template <typename T>
class best_line< std::vector< point<T> > > {
public:
	typedef std::vector< point<T> > sample;
public:
	best_line() { }
	best_line(sample const & s)
		{ start(s); }
public:
	void start(sample const & s) {
		channels.resize(s.size());
		for (size_t i = 0; i < channels.size(); ++i)
			channels[i].start(s[i]);
	}
	double finish(sample const & f) const {
		assert (f.size() == channels.size());
		double m = 0;
		for (size_t i = 0; i < channels.size(); ++i)
			m += channels[i].finish(f[i]);
		return m;
	}
	void intermediate(sample const & s) {
		assert (s.size() == channels.size());
		for (size_t i = 0; i < channels.size(); ++i)
			channels[i].intermediate(s[i]);
	}
	double intermediate_measure(sample const & f) {
		assert (f.size() == channels.size());
		double m = 0;
		for (size_t i = 0; i < channels.size(); ++i)
			m += channels[i].intermediate_measure(f[i]);
		return m;
	}
	double min_discrepancy() const {
		double m = 0;
		for (size_t i = 0; i < channels.size(); ++i)
			m += channels[i].min_discrepancy();
		return m;
	}
	double discrepancy_from_start() const {
		double m = 0;
		for (size_t i = 0; i < channels.size(); ++i)
			m += channels[i].discrepancy_from_start();
		return m;
	}
private:
	std::vector< best_line< point<T> > > channels;
};

/// replaces: 
/// 0 -> s, 1 -> s+1, 2 -> s+2, ... nums.size()-s-1 -> nums.size()-1
/// nums.size()-s -> 1, nums.size()-s+1 -> 2, ... nums.size()-1 -> s
void back_shift_numbers(std::vector<uint> & nums, uint s)
{
	uint m = nums.size();
	for (uint i = 0; i < m; ++i) {
		uint & n = nums[i];
		if (n < m-s)
			n += s;
		else {
			assert(n < m);
			n -= m-s-1;
		}
	}
}

/// fills preresized vector with consecutive numbers: 0, 1, 2, ...
inline void make_sequence(std::vector<uint> & nums)
{
	for (uint i = 0; i < nums.size(); ++i)
		nums[i] = i;
}

template <typename T>
static double simplify_min_step_len_core(int n, ///< min_step_len
	std::vector<T> const & src, std::vector<uint> & leaved_sample_numbers, bool closed)
{
	assert(n > 1);

	typedef std::vector<node_here> nodes;

	size_t const m = src.size();

	nodes ns(m);

	ns[0].d = 0;
	ns[0].prev_node = 0;

	size_t i;
	best_line<T> line;
	line.start(src[0]);
	for (i = 1; i < n && i < m; ++i) {
		ns[i].d = -1;
		ns[i].prev_node = 0;
		line.intermediate(src[i]);
	}
	for (; i < 2*n && i < m; ++i) {
		ns[i].d = line.intermediate_measure(src[i]);
		ns[i].prev_node = 0;
	}
	for (; i < m; ++i) {
		size_t max_j = (i < 3*n) ? i-n+1 : 2*n;
		line.start(src[i]);
		ns[i].d = std::numeric_limits<double>::max();
		size_t j;
		for (j = 1; j < n; ++j)
			line.intermediate(src[i-j]);
		for (; j < max_j; ++j) {
			assert(ns[i-j].d >= 0);
			double d = ns[i-j].d + line.intermediate_measure(src[i-j]);
			if (d < ns[i].d) {
				ns[i].d = d;
				ns[i].prev_node = i - j;

				//test for better start
				if (closed && j == 2*n-1) {
					int s = i - j;
					int c = 1;
					for (int k = 1; k < n; ++k)
						if (ns[i-k].prev_node == s)
							++c;
						else
							break;
					if (c == n) {
						//we found better fixed start point for closed curve
						ns.clear();
						std::vector<T> tmp;
						shift(src, tmp, s);
						double d = simplify_min_step_len_core(n, tmp, leaved_sample_numbers, 
							false //no closed flag because second start reselection is unwanted
						);
						back_shift_numbers(leaved_sample_numbers, s);
						return d;
					}
				}
			}
		}
		assert(ns[i].d != std::numeric_limits<double>::max());
	}

	//back pass
	std::vector<uint> tmp;
	tmp.push_back(m-1);
	i = m-1;
	while (i != 0) {
		node_here const & h = ns[i];
		i = h.prev_node;
		tmp.push_back(i);
	}
	leaved_sample_numbers.assign(tmp.rbegin(), tmp.rend());

	return ns[m-1].d;
}

template <typename T>
double simplify_min_step_len(int min_step_len,
	std::vector<T> const & src, std::vector<uint> & leaved_sample_numbers)
{
	if (min_step_len <= 1) {
		leaved_sample_numbers.resize(src.size());
		make_sequence(leaved_sample_numbers);
		return 0;
	}

	return simplify_min_step_len_core(min_step_len, src, leaved_sample_numbers, closed(src));
}

template <typename T>
double simplify_min_step_len(int min_step_len,
	std::vector<T> const & src, std::vector<T> & dst)
{
	std::vector<uint> leaved_sample_numbers;
	double d = simplify_min_step_len(min_step_len, src, leaved_sample_numbers);
	copy_samples(src, leaved_sample_numbers, dst);
	return d;
}

//

template <typename T>
static void make_first_layer(std::vector<T> const & src,
	std::vector<node_here> & first)
{
	best_line<T> line;
	int i;
	size_t m = first.size();

	first[0].d = -1;
	line.start(src[0]);
	for (i = 1; i < m-1; ++i) {
		first[i].d = line.intermediate_measure(src[i]);
		first[i].prev_node = 0;
	}
	if (closed(src))
		first[m-1].d = line.discrepancy_from_start();
	else
		first[m-1].d = line.intermediate_measure(src[m-1]);
	first[m-1].prev_node = 0;
}

template <typename T>
static void make_next_layer(std::vector<T> const & src,
	std::vector<node_here> const & last, std::vector<node_here> & next)
{
	best_line<T> line;
	int i, j;

	size_t m = last.size();
	next.resize(m);
	next[0].d = -1;
	for (i = 1; last[i-1].d < 0 && i < m; ++i)
		next[i].d = -1;
	for (; i < m; ++i) {
		line.start(src[i]);
		next[i].d = std::numeric_limits<double>::max();
		for (j = i-1; last[j].d >= 0; --j) {
			double d = last[j].d + line.intermediate_measure(src[j]);
			if (d < next[i].d) {
				next[i].prev_node = j;
				next[i].d = d;
			}
			else if (line.min_discrepancy() > next[i].d)
				break;
		}
		assert(next[i].d != std::numeric_limits<double>::max());
	}
}

///builds optimal coutour in reverse order by passing through layers
template <typename T>
static void layers_back_pass(
	std::vector<std::vector<node_here> > const & layers, int last_step,
	std::vector<T> & reverse_numbers)
{
	int i = last_step;
	int k = layers.size() - 1;
	while (i != 0) {
		reverse_numbers.push_back(i);
		i = layers[k][i].prev_node;
		--k;
	}
	assert(k == -1);
	reverse_numbers.push_back(0);
}

template <typename T>
static double simplify_to_K_nodes_core(int K,
	std::vector<T> const & src, std::vector<uint> & leaved_sample_numbers, bool closed)
{ 
	assert (K >= 2);
	size_t const m = src.size();
	assert (m >= 2);

	int i, k;
	best_line<T> line;

	if (K == 2) {
		//solution is known, we have to calculate discrepancy only
		line.start(src[0]);
		for (i = 1; i < m-1; ++i)
			line.intermediate(src[i]);
		double d = line.finish(src[m-1]);

		leaved_sample_numbers.resize(2);
		leaved_sample_numbers[0] = 0;
		leaved_sample_numbers[1] = 1;
		return d;
	}

	typedef std::vector<node_here> nodes;
	std::vector<nodes> layers(K-2);

	layers[0].resize(m);
	make_first_layer(src, layers[0]);

	for (k = 1; k < K-2; ++k) 
		make_next_layer(src, layers[k-1], layers[k]);

	//last step
	int ls;
	double lsd = std::numeric_limits<double>::max();
	line.start(src[m-1]);
	for (i = m-2; layers[K-3][i].d >= 0; --i) {
		double d = layers[K-3][i].d + line.intermediate_measure(src[i]);
		if (d < lsd) {
			ls = i;
			lsd = d;
		}
	}
	assert(lsd != std::numeric_limits<double>::max());

	if (closed && K >= 3) {
		//how many times node is leaved in contour,
		//summed for simplified contours of size 3..K
		std::vector<int> stat(m);
		for (int j = 1; j <= K-3; ++j) {
			if (j == K-3)
				i = ls;
			else
				i = layers[j+1][m-1].prev_node;
			k = j;
			while (i != 0) {
				++stat[i];
				i = layers[k][i].prev_node;
				--k;
			}
			assert(k == -1);
		}
		int s = std::max_element(stat.begin(), stat.end()) - stat.begin();

		//new fixed start point for closed curve is choosen
		stat.clear();
		layers.clear();
		std::vector<T> tmp;
		shift(src, tmp, s);
		double d = simplify_to_K_nodes_core(K, tmp, leaved_sample_numbers, 
			false //no closed flag because second start reselection is unwanted
		);
		back_shift_numbers(leaved_sample_numbers, s);
		return d;
	}

	//back pass
	std::vector<uint> tmp;
	tmp.push_back(m-1);
	layers_back_pass(layers, ls, tmp);
	leaved_sample_numbers.assign(tmp.rbegin(), tmp.rend());

	return lsd;
}

template <typename T>
double simplify_to_K_nodes(int K,
	std::vector<T> const & src, std::vector<uint> & leaved_sample_numbers)
{
	if (K <= 1) {
		leaved_sample_numbers.resize(src.size());
		make_sequence(leaved_sample_numbers);
		return 0;
	}

	return simplify_to_K_nodes_core(K, src, leaved_sample_numbers, closed(src));
}


template <typename T>
double simplify_to_K_nodes(int K,
	std::vector<T> const & src, std::vector<T> & dst)
{
	std::vector<uint> leaved_sample_numbers;
	double d = simplify_to_K_nodes(K, src, leaved_sample_numbers);
	copy_samples(src, leaved_sample_numbers, dst);
	return d;
}

//

template <typename T>
static double simplify_max_discrepancy_core(double max_discrepancy,
	std::vector<T> const & src, std::vector<uint> & leaved_sample_numbers, bool closed)
{
	assert(max_discrepancy > 0);
	size_t const m = src.size();
	assert (m >= 2);

	best_line<T> line;

	typedef std::vector<node_here> nodes;
	std::vector<nodes> layers(1);
	layers.reserve(m); //no resizing is permitted

	layers[0].resize(m);
	make_first_layer(src, layers[0]);

	int k;
	for (k = 0; layers[k][m-1].d > max_discrepancy; ++k) {
		layers.push_back(nodes());
		make_next_layer(src, layers[k], layers[k+1]);
	}

	if (closed && k >= 1) {
		//how many times node is leaved in contour,
		//summed for simplified contours of arbitrary size
		std::vector<int> stat(m);
		for (int j = 1; j <= k; ++j) {
			int i = layers[j][m-1].prev_node;
			int l = j-1;
			while (i != 0) {
				++stat[i];
				i = layers[l][i].prev_node;
				--l;
			}
			assert(l == -1);
		}
		int s = std::max_element(stat.begin(), stat.end()) - stat.begin();

		//new fixed start point for closed curve is choosen
		stat.clear();
		layers.clear();
		std::vector<T> tmp;
		shift(src, tmp, s);
		double d = simplify_max_discrepancy_core(max_discrepancy, tmp, leaved_sample_numbers, 
			false //no closed flag because second start reselection is unwanted
		);
		back_shift_numbers(leaved_sample_numbers, s);
		return d;
	}

	//back pass
	std::vector<uint> tmp;
	layers_back_pass(layers, m-1, tmp);
	leaved_sample_numbers.assign(tmp.rbegin(), tmp.rend());

	return layers[k][m-1].d;
}

template <typename T>
double simplify_max_discrepancy(double max_discrepancy,
	std::vector<T> const & src, std::vector<uint> & leaved_sample_numbers)
{
	if (max_discrepancy == 0) {
		leaved_sample_numbers.resize(src.size());
		make_sequence(leaved_sample_numbers);
		return 0;
	}
	return simplify_max_discrepancy_core(max_discrepancy, src, leaved_sample_numbers, closed(src));
}

template <typename T>
double simplify_max_discrepancy(double max_discrepancy,
	std::vector<T> const & src, std::vector<T> & dst)
{
	std::vector<uint> leaved_sample_numbers;
	double d = simplify_max_discrepancy(max_discrepancy, src, leaved_sample_numbers);
	copy_samples(src, leaved_sample_numbers, dst);
	return d;
}

#define INSTANTIATE(T) \
template double simplify_min_step_len(int min_step_len, \
	std::vector< T > const & src, std::vector< T > & dst); \
template double simplify_to_K_nodes(int K, \
	std::vector< T > const & src, std::vector< T > & dst); \
template double simplify_max_discrepancy(double max_discrepancy, \
	std::vector< T > const & src, std::vector< T > & dst);

INSTANTIATE(point_us)
INSTANTIATE(point_s)
INSTANTIATE(point_i)
INSTANTIATE(point_f)
INSTANTIATE(point_d)

INSTANTIATE(std::vector<point_us>)
INSTANTIATE(std::vector<point_s>)
INSTANTIATE(std::vector<point_i>)
INSTANTIATE(std::vector<point_f>)
INSTANTIATE(std::vector<point_d>)

} //namespace outline
